#ifndef _UTILS_H
#define _UTILS_H    1

/* 需要的c标准库和os相关库 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <errno.h>
#include <assert.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <arpa/inet.h>

/* 常用宏定义 */
#define sign(x)      (x>0?1:(x<0?-1:0))
#define d2r(x)       (x/180.0*M_PI)
#define r2d(x)       (x/M_PI*180.0)
#define threshold(x) (fabs(x)<0.00001)
#define min(a,b)     (((a)>(b))?(a):(b))
#define print(x)     (printf("%s\n",x))
#define enter        (printf("\n"))
#define pf(x)        (printf("%lf\n",x))
#define cmdis(x)     (!strcmp(buf,x))
#define HEIGHT 	     165.40

double Dist(double *p1, double *p2)
{
	return sqrt(pow(p1[0]-p2[0], 2) + pow(p1[1]-p2[1], 2) + pow(p1[2]-p2[2], 2));
}

/* 调用linux的wc命令统计文件行数 */
int wc_l(const char *fname)
{
	char buf[256] = {0};
	sprintf(buf, "wc -l %s", fname);
	FILE *fp = NULL;
	if ((fp=popen(buf,"r")))
	{ //判断语句中使用=赋值时加上括号
		memset(buf, 0, sizeof(buf));
		if (fgets(buf, sizeof(buf), fp))
		{
			pclose(fp);
			int ind = strchr(buf, ' ') - buf;
			buf[ind] = '\0';
			return atoi(buf);
		}
		pclose(fp);
	}
	return -1;
}

/* 读取关节路径(角度值) */
void ReadPath(const char *FileName, int LineNum, double *data)
{
	FILE *rfp = fopen(FileName, "r");
	char tmp[100];

	for (int i=0; i<LineNum; i++)
	{
		for (int j=0; j<6; j++)
		{
			fscanf(rfp, "%s", tmp);
			data[i*6+j] = d2r(atof(tmp));
		}
	}

	fclose(rfp);
}

typedef struct {
	double *elem;
	int row, col;
} Matrix;

Matrix* initm(int row, int col)
{
	if (row>0 && col>0)
	{
		Matrix *matrix = (Matrix*)malloc(sizeof(Matrix));
		matrix->row = row;
		matrix->col = col;
		matrix->elem = (double*)malloc(sizeof(double)*row*col);
		memset(matrix->elem, 0, sizeof(double)*row*col);
		return matrix;
	}
	else 
		return NULL;
}

void valuem(Matrix *matrix, double *array)
{
	if (matrix->elem != NULL)
	{
		memcpy(matrix->elem, array, matrix->row*matrix->col*sizeof(double));
	}
}

int sizeofm(Matrix *matrix)
{
	return matrix->row*matrix->col;
}

void freem(Matrix *matrix)
{
	free(matrix->elem);
	free(matrix);
}

void copym(Matrix *matrix_A, Matrix *matrix_B)
{
	matrix_B->row = matrix_A->row;
	matrix_B->col = matrix_A->col;
	memcpy(matrix_B->elem, matrix_A->elem, sizeofm(matrix_A)*sizeof(double));
}

void printm(Matrix *matrix)
{
	for (int i=0; i<sizeofm(matrix); i++)
	{
		printf("%lf\t", matrix->elem[i]);
		if ((i+1)%matrix->col == 0)
		{
			printf("\n");
		}
	}
			
}

Matrix* addm(Matrix *matrix_A,Matrix *matrix_B)
{
	if (matrix_A->row == matrix_B->row && matrix_A->col == matrix_B->col)
	{
		Matrix *matrix_C = initm(matrix_A->row,matrix_A->col);
		for (int i=0; i<matrix_A->col; i++)
		{
			for (int j=0; j<matrix_A->row; j++)
			{
				matrix_C->elem[i*matrix_C->row + j] = matrix_A->elem[i*matrix_A->row + j] + matrix_B->elem[i*matrix_A->row + j];
			}
		}
		return matrix_C;
	}
	else 
	{
		printf("不可相加\n");
		return NULL;
	}
}

Matrix* mulm(Matrix *matrix_A,Matrix *matrix_B)
{
	if (matrix_A->row == matrix_B->col)		//列==行
	{
		Matrix *matrix_C = initm(matrix_B->row,matrix_A->col);
		// matrix_C->col = matrix_A->col;		//A行
		// matrix_C->row = matrix_B->row;			//B列
		for (int i=0;i<matrix_A->row;i++)
		{
			for (int j=0;j<matrix_B->col;j++)
			{
				for (int k=0;k<matrix_A->col;k++)
				{
					matrix_C->elem[i*matrix_C->col + j] += \
					matrix_A->elem[i*matrix_A->col + k] * matrix_B->elem[k*matrix_B->row + j];
				}
			}
		}
		return matrix_C;
	}
	else
	{
		printf("不可相乘\n");
		return NULL;
	}
}

void tranm(Matrix *matrix)
{
	if (matrix->row == matrix->col)
	{
		Matrix *matrixTemp = initm(matrix->row, matrix->col);
		copym(matrix, matrixTemp);

		for (int i=0; i<matrix->row; i++)
		{
			for (int j=0; j<matrix->col; j++)
			{
				matrix->elem[i*matrix->row + j] = matrixTemp->elem[j*matrix->row + i];
			}
		}
	}
	else
	{
		printf("转置的矩阵必须为方阵\n");
	}
}

// string get_time()
// {
// 	time_t timep;
// 	time(&timep);
// 	char tmp[64];
// 	strftime(tmp, sizeof(tmp), "%Y/%m/%d %H:%M:%S", localtime(&timep));
// 	return tmp;
// }

#endif /* <utils.h> included.  */